/*
 * Wander.c
 *
 *  Created on: Apr 8, 2014
 *      Author: laventine5678
 Edited: kvnsmith
 */

#include "Wander.h"
int lm;
int rm;

int main() {
    setup();
    updateMotorValues();

    //run
    for (;;) {
        if (CHECK_SONAR) { //every 1/10th of a second
            detectCollision();
            CHECK_SONAR = 0;
        } else {
            if (R_FLAG) {  //every 1/2 s
                updateMotorValues();
                R_FLAG = 0;
            }
            wander(lm, rm);
        }
    }
}

void sonarISR() { //every 1/10th of a second
    CHECK_SONAR = 1;
    TIMER++;
    if ((TIMER % 5) == 0) { //every 1/2 of a second
        R_FLAG = 1;
    }
}

void wander(int lmotor, int rmotor) {
    if (GO_FLAG) {
        softPwmWrite(R_MOTOR_PIN, rmotor);
        softPwmWrite(L_MOTOR_PIN, lmotor);
    }
}

void updateMotorValues() {
    int amplitude = rand() % 10;
    if (direction > 0) {
        //turn on pin 1 for LED indication
        digitalWrite(4, 0);
        digitalWrite(1, 1);
        rm = amplitude + 13;
        lm = 0;
    } else {
        //turn on pin 1 for LED indication
        digitalWrite(1, 0);
        digitalWrite(4, 1);
        lm = amplitude + 32;
        rm = 0;
    }
    direction *= -1;
}

void detectCollision() {
    float r_inches, l_inches;
    //Right sensor
    pinMode(R_SENSOR_PIN, OUTPUT);
    sendPulse(R_SENSOR_PIN);
    pinMode(R_SENSOR_PIN, INPUT);


    // distance of pulse from object detected to the sonar sensor
    // right_duration = pulseIn(R_SENSOR_PIN, HIGH, 5000);
    // convert the time into a distance
    r_inches = microsecondsToInches(pulseIn(R_SENSOR_PIN, HIGH, 5000));
    printf("The object distance from RIGHT side:%2.2f inches\n", r_inches);

    delay(13);
    //left sensor
    pinMode(L_SENSOR_PIN, OUTPUT);
    sendPulse(L_SENSOR_PIN);
    pinMode(L_SENSOR_PIN, INPUT);

    l_inches = microsecondsToInches(pulseIn(L_SENSOR_PIN, HIGH, 5000));

    delay(13);


    printf("The object distance from LEFT side:%2.2f inches\n", l_inches);

    if ((l_inches <= 25 && l_inches > 1) || (r_inches <= 25 && r_inches > 1)) {
        //Stop if object seen
        softPwmWrite(L_MOTOR_PIN, 0);
        softPwmWrite(R_MOTOR_PIN, 0);
        GO_FLAG = 0;
    } else {
        GO_FLAG = 1;
    }



    //------------------------PSEUDOCODE FOR MULTIPLE SENSORS------------------------//
    //int sensor_i_inches.....;
    //for (i sensor_i = 0; sensor_i < all_sensors; sensor_i++)
    //pinMode(sensor_i_PIN, OUTPUT)
    //sendPulse(sensor_i_pin);
    //pinMode(sensor_i_PIN, INPUT)
    //sensor_i_inches = microsecondsToInches(pulseIn(sensor_i_pin, HIGH, 5000));

    //delay(100);
    //if (if sensor_i is close to an object)
    //change direction by changing L_PWM or R_PWM
    //GO_FLAG = 1;
    //else if (all sensor_i's is close to an object)
    //GO_FLAG = 0
    //else
    //GO_FLAG = 1
    //------------------------------------------------------------------------------------//
}


void setup() {
    //setup wiringPi library
    if (wiringPiSetup() == -1) {
        printf("Fail to setup wiringPi \n");
        exit(1);
    }

    //setup timer interrupt
    //timer_pin is activated every 1/10s
    if (wiringPiISR(TIMER_PIN, INT_EDGE_FALLING, &sonarISR) < 0) {
        printf("Fail to setup ISR \n");
        exit(1);
    }

    //initialize all pins
    pinMode(R_MOTOR_PIN, OUTPUT);
    pinMode(L_MOTOR_PIN, OUTPUT);
    pinMode(LEFT_LED_PIN, OUTPUT);
    pinMode(RIGHT_LED_PIN, OUTPUT);

    //initialize PWM values on left motor pins
    //int pin, int initialValue, int pwmRange
    if (softPwmCreate(L_MOTOR_PIN, 0, PWM_RANGE) == -1) {
        printf("Fail to create softPwm \n");
        exit(1);
    }

    //initialize PWM values on left motor pins
    if (softPwmCreate(R_MOTOR_PIN, 0, PWM_RANGE) == -1) {
        printf("Fail to create softPwm \n");
        exit(1);
    }
}



void sendPulse(int sensorPin) {
    //Right sensor
    digitalWrite(sensorPin, LOW);
    delayMicroseconds(2);
    digitalWrite(sensorPin, HIGH);
    delayMicroseconds(5);
    digitalWrite(sensorPin, LOW);
}


//Formula provided by Radio Shack
long microsecondsToInches(long microseconds) {
    return microseconds / 74 / 2;
}

//Provided by Raspberry Pi enthusiast
int pulseIn(int pin, int level, int timeout) {
    struct timeval tn, t0, t1;
    long micros;
    gettimeofday(&t0, NULL);
    micros = 0;
    while (digitalRead(pin) != level) {
        gettimeofday(&tn, NULL);
        if (tn.tv_sec > t0.tv_sec)
            micros = 1000000L;
        else
            micros = 0;
        micros += (tn.tv_usec - t0.tv_usec);
        if (micros > timeout)
            return 0;
    }
    gettimeofday(&t1, NULL);
    while (digitalRead(pin) == level) {
        gettimeofday(&tn, NULL);
        if (tn.tv_sec > t0.tv_sec)
            micros = 1000000L;
        else
            micros = 0;
        micros = micros + (tn.tv_usec - t0.tv_usec);
        if (micros > timeout)
            return 0;
    }
    if (tn.tv_sec > t1.tv_sec)
        micros = 1000000L;
    else
        micros = 0;
    micros = micros + (tn.tv_usec - t1.tv_usec);
    return micros;
}
